Прямая задача кинематики PUMA

from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook

from kinematics import Vector, Quaternion, Transform
import graphics

Решение прямой задачи кинематики для манипулятора PUMA

Манипулятор кинематической схемы PUMA обладает шестью степенями подвижности.

Его можно условно разделить на сегменты, соединияюще между собой:

• основание
• плечо
• локоть
• кисть
• фланец

Длина этих сегментов определена в таблице:

Пара	длина
основание - плечо	$l_0$
плечо - локоть	$l_1$
локоть - кисть	$l_2$
кисть - фланец	$l_3$

def puma_chain(q, l, math_source=np):
    base = Transform.identity()
    shoulder = base + Transform(
        Vector(0, 0, l[0]),
        Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source) * 
        Quaternion.from_angle_axis(q[1], Vector(0, 1, 0), math_source)
    )
    elbow = shoulder + Transform(
        Vector(0, 0, l[1]),
        Quaternion.from_angle_axis(q[2], Vector(0, 1, 0), math_source)
    )
    wrist = elbow + Transform(
        Vector(0, 0, l[2]),
        Quaternion.from_angle_axis(q[3], Vector(0, 0, 1), math_source) *
        Quaternion.from_angle_axis(q[4], Vector(0, 1, 0), math_source)
    )
    flange = wrist + Transform(
        Vector(0, 0, l[3]),
        Quaternion.from_angle_axis(q[5], Vector(0, 0, 1), math_source)
    )
    return [base, shoulder, elbow, wrist, flange]

Зададим изменение обобщенных координат:

def puma_q(t, total):
    #omega = t / total * np.pi * 2
    a = (t - total) / 2
    omega = (-np.cos(3.14 / total * t) + 1.0)*5

    return [
        np.pi / 4 * np.cos(omega),
        np.pi / 6* np.sin(omega),
        np.pi / 2,
        np.pi*3/4,
        np.pi *np.sin(omega)/ 6,
        0
    ]

Зададим длины звеньев:

puma_l = [1, 2, 1, 0.5]

puma_fig = plt.figure()
ax = puma_fig.add_subplot(projection="3d")
ax.set_xlim([-2, 2]); ax.set_ylim([-2, 2]); ax.set_zlim([0, 4])
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)

total = 100

def animate(frame):
    chain = puma_chain(puma_q(frame, total), puma_l)
    (x, y, z) = graphics.chain_to_points(chain)
    lines.set_data_3d(x, y, z)
    global r, g, b
    r.remove(); g.remove(); b.remove()
    r, g, b = graphics.axis(ax, chain[-1], 0.5)

    
animate(0)
fps = 25
puma_ani = animation.FuncAnimation(
    puma_fig,
    animate,
    frames=total,
    interval=1000.0/fps
)

HTML(puma_ani.to_jshtml())

Once Loop Reflect

Отображение траектории

t_end = 10
step = 0.1
t = np.arange(0, t_end, step)
chain = puma_chain(puma_q(t, t_end), puma_l)
x, y, z = graphics.chain_to_points(chain)

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.set_xlim([-2, 2]); ax.set_ylim([-2, 2]); ax.set_zlim([0, 4])
ax.plot(x[2], y[2], z[2], color="#c0c0c0")
ax.plot(x[3], y[3], z[3], color="#c0c0c0")
ax.plot(x[4], y[4], z[4], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
fig.show()
print(
    "X координата менялась в диапазоне от",
    np.min(x[-1]),
    "до",
    np.max(x[-1]),
        "\nY координата менялась в диапазоне от",
    np.min(y[-1]),
    "до",
    np.max(y[-1]),
        "\nZ координата менялась в диапазоне от",
    np.min(z[-1]),
    "до",
    np.max(z[-1])
)

X координата менялась в диапазоне от 0.32187772587569363 до 2.334022433636939 
Y координата менялась в диапазоне от -1.1898168635068487 до 1.3119564153188643 
Z координата менялась в диапазоне от 2.168650051278921 до 3.2961913036419443

Измерение скорости движения

Численно продиффиринцируем положение всех точек траектории

t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = puma_chain(puma_q(t, t_end), puma_l)
end = chain[-1]
velocity_x = np.diff(end.translation.x) / step
velocity_y = np.diff(end.translation.y) / step
velocity_z = np.diff(end.translation.z) / step
velocity_total = (velocity_x ** 2 + velocity_y ** 2 + velocity_z ** 2) ** 0.5

fig = plt.figure()
ax = fig.add_subplot()

ax.plot(t[:-1], velocity_x, color="#ff0000", label="$V_x$")
ax.plot(t[:-1], velocity_y, color="#00ff00", label="$V_y$")
ax.plot(t[:-1], velocity_z, color="#0000ff", label="$V_z$")
ax.plot(t[:-1], velocity_total, color="#000000", label="$V$")
fig.legend()
fig.show()

index_max = np.argmax(velocity_total)
vx_max = np.argmax(velocity_x)
vy_max = np.argmax(velocity_y)
vz_max = np.argmax(velocity_z)
print(
    "Максимальная скорость была",    velocity_total[index_max],    "в момент",    index_max * step,    "сек\n",    
    "Максимальная скорость по оси X была",    velocity_x[vx_max],    "в момент",    vx_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_y[vy_max],    "в момент",    vy_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_z[vz_max],    "в момент",    vz_max * step,    "сек\n"
)

Максимальная скорость была 2.379054911697325 в момент 6.8500000000000005 сек
 Максимальная скорость по оси X была 1.5236329096548484 в момент 6.33 сек
 Максимальная скорость по оси Y была 1.689859544573824 в момент 5.55 сек
 Максимальная скорость по оси Y была 1.0259567021000837 в момент 3.5100000000000002 сек

Самостоятельные задания

Для своего закона изменения обобщенных координат

• Оцените диапазон $X$, $Y$ и $Z$ координат для данной траектории;
• Оцените максимальную скорость по осям;
• Оцените абсолютную максимальную скорость.